#include "gnss_imu_fusion/fusion/gnss_imu_fusion.h"
#include <color_terminal/color_terminal.hpp>
GNSSImuFusion::GNSSImuFusion(/* args */)
{
    auto p = eskf.getP();
    p.setIdentity();
    p(9,9) = p(10,10) = p(11,11) = 0.001;
    p(12,12) = p(13,13) = p(14,14) = 0.001;
    p(15,15) = p(16,16) = p(17,17) = 0.001; 
    p/=100;
    eskf.setP(p);
    auto x = eskf.getX();
    auto q =  eskf.getQ();
    x.ba.setZero();
    x.bg.setZero();
    x.gravity.setZero();
    // x.gravity[2] = SlamCraft::GRAVITY;
    x.position.setZero();
    x.velocity.setZero();
    x.rotation.setIdentity();
        ///. Q
    Eigen::Vector3d cov_gyr = {0.01,0.01,0.01};
    Eigen::Vector3d cov_acc = cov_gyr;
    Eigen::Vector3d cov_bias_acc = {0.0001,0.0001,0.0001};
    Eigen::Vector3d cov_bias_gyr = cov_bias_acc;

    q.setZero();
    q.block<3, 3>(0, 0).diagonal() = cov_gyr;
    q.block<3, 3>(3, 3).diagonal() = cov_acc;
    q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
    q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
    eskf.setX(x);
    eskf.setQ(q);
    eskf.zhr_model = std::bind(&GNSSImuFusion::calc_eskf_zhr,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);

}

GNSSImuFusion::~GNSSImuFusion()
{

}

void GNSSImuFusion::calc_eskf_zhr(const SlamCraft::ESKF::State_18& X,Eigen::MatrixXd & Z,Eigen::MatrixXd & H,Eigen::MatrixXd & R){
    Eigen::Vector3d now_gnss_postion;
    auto q = X.rotation;
    ez_geo.get_relative_position(now_obs.gnss,now_gnss_postion);
    Z.resize(3,1);
    Z = now_gnss_postion-X.position;
    H.resize(3,18);
    H.setZero();
    H.block<3,3>(0,3).setIdentity();
}

Report GNSSImuFusion::add(const SlamCraft::IMU&imu){
    Report report;
    if(!is_inited){
        imu_buffer.push_back(imu);
        eskf_init();
        return report;
    }
    auto avg_imu = (last_imu+imu)*0.5;
    
    if(obs_buffer.empty()||imu.stamp<obs_buffer.front().stamp){
        double dt = imu.stamp-last_imu.stamp;
        eskf.predict(avg_imu,dt);
        report.is_predict = true;
        report.predict_state = eskf.getX();
        report.predict_time = imu.stamp;
        
    }else{
        assert(last_imu.stamp<obs_buffer.front().stamp&&imu.stamp>obs_buffer.front().stamp);
        
        double dt = obs_buffer.front().stamp-last_imu.stamp;
        eskf.predict(avg_imu,dt);
        now_obs = obs_buffer.front();
        eskf.update();
        
        report.is_obs_update = true;
        report.obs_update_state = eskf.getX();
        report.obs_update_time = now_obs.stamp;
        
        eskf.predict(avg_imu,imu.stamp-obs_buffer.front().stamp);
        
        report.is_predict = true;
        report.predict_state = eskf.getX();
        report.predict_time = imu.stamp;
        
        eskf_state_cache.clear();
        last_imu.stamp = now_obs.stamp;
        obs_buffer.pop_front();

    }
    ESKFStateCacheItem cache;
    cache.imu = avg_imu;
    cache.imu_end_time = imu.stamp;
    cache.imu_start_time = last_imu.stamp;
    cache.P = eskf.getP();
    cache.state = eskf.getX();
    eskf_state_cache.push_back(cache);
    last_imu = imu;
    return report;
}
Report GNSSImuFusion::add(const Obs&obs){
    static ESKFStateCacheItem last_cache;
    Report report;
    if(!is_inited){
        obs_buffer.push_back(obs);
        eskf_init();
        last_cache.P = eskf.getP();
        last_cache.state = eskf.getX();
        // std::cout<<"broken 1 not_init"<<std::endl;
        return report;
    }


    if(obs.stamp<eskf_state_cache.front().imu_start_time){
        // std::cout<<"broken 2  obs too old"<<std::endl;
        return report;
    }
    while (!eskf_state_cache.empty()&&eskf_state_cache.front().imu_end_time<obs.stamp){
        last_cache = eskf_state_cache.front();
        eskf_state_cache.pop_front();
    }
    if(eskf_state_cache.empty()){
        // std::cout<<"broken 3  imu too old"<<std::endl;
        obs_buffer.push_back(obs);
        return report;
    }
    eskf.setP(last_cache.P);
    eskf.setX(last_cache.state);
    eskf.predict(eskf_state_cache.front().imu,obs.stamp-eskf_state_cache.front().imu_start_time);
    now_obs= obs;
    eskf.update();

    report.is_obs_update = true;
    report.obs_update_state = eskf.getX();

    // . 重新预测(重新进行前向过程)
    eskf_state_cache.front().imu_start_time = obs.stamp;
    for (auto &&cache : eskf_state_cache)
    {
        eskf.predict(cache.imu,cache.imu_end_time-cache.imu_start_time);
        cache.P = eskf.getP();
        cache.state = eskf.getX();
    }
    return report;
}
void GNSSImuFusion::eskf_init(){
    // 获取一组数据
    if (is_inited)return;
    if (obs_buffer.empty()||imu_buffer.empty())
    {
        return ;
    }
    while (!obs_buffer.empty()&&obs_buffer.front().stamp<imu_buffer.front().stamp)
    {
        obs_buffer.pop_front();
    }
    if (obs_buffer.empty())
    {
        return  ;
    }
    
    if (obs_buffer.front().stamp>imu_buffer.back().stamp)
    {
        return  ;
    }
    auto obs = obs_buffer.front();
    std::vector<SlamCraft::IMU> imus;
    while (imu_buffer.front().stamp<obs.stamp)
    {
        imus.push_back(imu_buffer.front());
        imu_buffer.pop_front();
    }
    obs_buffer.pop_front();
    // 初始化
    static int imu_count = 0;
    for (size_t i = 0; i <imus.size(); i++)
    {
        imu_count++;
        auto x = eskf.getX();
        x.gravity += imus[i].acc;
        x.bg += imus[i].gro;
        x.rotation = imus[i].rot;
        eskf.setX(x);

    }
    if (imu_count >= 5)
    {
        auto x = eskf.getX();
        x.gravity /=double(imu_count);

        x.bg /=double(imu_count);
        x.bg.setZero();
        x.gravity =  -1*x.gravity ;
        eskf.setX(x);
        is_inited = true;
        obs_buffer.clear();
        imu_buffer.clear();
    }
    last_imu = imus.back();
    last_imu.stamp = obs.stamp;
    ez_geo.reset_gnss_pose(obs.gnss);
}